﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using GLWrapper;

namespace GameBaseCode
{
    // Adapted from the Lighthouse3D.com tutorial
    public class Frustum
    {
        private Plane[] pl = new Plane[6];
        enum PLANES : int
        {
            TOP = 0, BOTTOM, LEFT,
            RIGHT, NEARP, FARP
        };

        public enum FrustumResult:int{OUTSIDE, INTERSECT, INSIDE};

        private Vector3 ntl;
        private Vector3 ntr;
        private Vector3 nbl;
        private Vector3 nbr;

        private Vector3 ftl;
        private Vector3 ftr;
        private Vector3 fbl;
        private Vector3 fbr;

        float nearD, farD, ratio, angle, tang;
        float nw, nh, fw, fh;

        private static double ANG2RAD = Math.PI / 180.0;

        public Frustum()
        {
            for (int i = 0; i < 6; i++)
            {
                pl[i] = new Plane();
            }
        }

        public void setCamInternals(float angle, float ratio, float nearD, float farD) {

	        this.ratio = ratio;
	        this.angle = angle;
	        this.nearD = nearD;
	        this.farD = farD;

	        tang = (float)Math.Tan(angle* ANG2RAD * 0.5) ;
	        nh = nearD * tang;
	        nw = nh * ratio; 
	        fh = farD  * tang;
	        fw = fh * ratio;
        }

        public void setCamDef(Vector3 p, Vector3 l, Vector3 u)
        {
	        Vector3 nc,fc,X,Y,Z;

	        Z = p - l;
	        Z.normalize();

	        X = Vector3.CrossProduct(u,Z);
	        X.normalize();

            Y = Vector3.CrossProduct(Z, X);

	        nc = p - Z * nearD;
	        fc = p - Z * farD;

	        ntl = nc + Y * nh - X * nw;
	        ntr = nc + Y * nh + X * nw;
	        nbl = nc - Y * nh - X * nw;
	        nbr = nc - Y * nh + X * nw;

	        ftl = fc + Y * fh - X * fw;
	        ftr = fc + Y * fh + X * fw;
	        fbl = fc - Y * fh - X * fw;
	        fbr = fc - Y * fh + X * fw;

	        pl[(int)PLANES.TOP].set3Points(ntr,ntl,ftl);
            pl[(int)PLANES.BOTTOM].set3Points(nbl, nbr, fbr);
            pl[(int)PLANES.LEFT].set3Points(ntl, nbl, fbl);
            pl[(int)PLANES.RIGHT].set3Points(nbr, ntr, fbr);
            pl[(int)PLANES.NEARP].set3Points(ntl, ntr, nbr);
            pl[(int)PLANES.FARP].set3Points(ftr, ftl, fbl);
        }

        public FrustumResult sphereInFrustum(Vector3 p, float raio)
        {
	        FrustumResult res = FrustumResult.INSIDE;
	        float distance;

	        for(int i=0; i < 6; i++) {
		        distance = pl[i].distanceTo(p);
		        if (distance < -raio)
                    return FrustumResult.OUTSIDE;
		        else if (distance < raio)
                    res = FrustumResult.INTERSECT;
	        }
	        return(res);
        }

        public void drawLines()
        {
            GL.glBegin(GL.GL_LINE_LOOP);
            //near plane
            GL.glVertex3f(ntl.x, ntl.y, ntl.z);
            GL.glVertex3f(ntr.x, ntr.y, ntr.z);
            GL.glVertex3f(nbr.x, nbr.y, nbr.z);
            GL.glVertex3f(nbl.x, nbl.y, nbl.z);
            GL.glEnd();

            GL.glBegin(GL.GL_LINE_LOOP);
            //far plane
            GL.glVertex3f(ftr.x, ftr.y, ftr.z);
            GL.glVertex3f(ftl.x, ftl.y, ftl.z);
            GL.glVertex3f(fbl.x, fbl.y, fbl.z);
            GL.glVertex3f(fbr.x, fbr.y, fbr.z);
            GL.glEnd();

            GL.glBegin(GL.GL_LINE_LOOP);
            //bottom plane
            GL.glVertex3f(nbl.x, nbl.y, nbl.z);
            GL.glVertex3f(nbr.x, nbr.y, nbr.z);
            GL.glVertex3f(fbr.x, fbr.y, fbr.z);
            GL.glVertex3f(fbl.x, fbl.y, fbl.z);
            GL.glEnd();

            GL.glBegin(GL.GL_LINE_LOOP);
            //top plane
            GL.glVertex3f(ntr.x, ntr.y, ntr.z);
            GL.glVertex3f(ntl.x, ntl.y, ntl.z);
            GL.glVertex3f(ftl.x, ftl.y, ftl.z);
            GL.glVertex3f(ftr.x, ftr.y, ftr.z);
            GL.glEnd();

            GL.glBegin(GL.GL_LINE_LOOP);
            //left plane
            GL.glVertex3f(ntl.x, ntl.y, ntl.z);
            GL.glVertex3f(nbl.x, nbl.y, nbl.z);
            GL.glVertex3f(fbl.x, fbl.y, fbl.z);
            GL.glVertex3f(ftl.x, ftl.y, ftl.z);
            GL.glEnd();

            GL.glBegin(GL.GL_LINE_LOOP);
            // right plane
            GL.glVertex3f(nbr.x, nbr.y, nbr.z);
            GL.glVertex3f(ntr.x, ntr.y, ntr.z);
            GL.glVertex3f(ftr.x, ftr.y, ftr.z);
            GL.glVertex3f(fbr.x, fbr.y, fbr.z);

            GL.glEnd();

        }

        public void drawNormals()
        {
            GL.glBegin(GL.GL_LINES);
            float arrowLen = 10.0f;
            // near
            Vector3 a = (ntr + ntl + nbr + nbl) * 0.25f;
            Vector3 b = a + pl[(int)PLANES.NEARP].normal * arrowLen;
            GL.glVertex3f(a.x, a.y, a.z);
            GL.glVertex3f(b.x, b.y, b.z);

            // far
            a = (ftr + ftl + fbr + fbl) * 0.25f;
            b = a + pl[(int)PLANES.FARP].normal * arrowLen;
            GL.glVertex3f(a.x, a.y, a.z);
            GL.glVertex3f(b.x, b.y, b.z);

            // left
            a = (ftl + fbl + nbl + ntl) * 0.25f;
            b = a + pl[(int)PLANES.LEFT].normal * arrowLen;
            GL.glVertex3f(a.x, a.y, a.z);
            GL.glVertex3f(b.x, b.y, b.z);

            // right
            a = (ftr + nbr + fbr + ntr) * 0.25f;
            b = a + pl[(int)PLANES.RIGHT].normal * arrowLen;
            GL.glVertex3f(a.x, a.y, a.z);
            GL.glVertex3f(b.x, b.y, b.z);

            // top
            a = (ftr + ftl + ntr + ntl) * 0.25f;
            b = a + pl[(int)PLANES.TOP].normal * arrowLen;
            GL.glVertex3f(a.x, a.y, a.z);
            GL.glVertex3f(b.x, b.y, b.z);

            // bottom
            a = (fbr + fbl + nbr + nbl) * 0.25f;
            b = a + pl[(int)PLANES.BOTTOM].normal * arrowLen;
            GL.glVertex3f(a.x, a.y, a.z);
            GL.glVertex3f(b.x, b.y, b.z);

            GL.glEnd();
        }
        public void drawPlanes()
        {
            GL.glEnable(GL.GL_BLEND);
            GL.glBlendFunc(GL.GL_SRC_ALPHA, GL.GL_ONE_MINUS_SRC_ALPHA);
            GL.glColor4f(0, 0, 0.5f, 0.5f);
            GL.glBegin(GL.GL_QUADS);

            //near plane
            GL.glVertex3f(ntl.x, ntl.y, ntl.z);
            GL.glVertex3f(ntr.x, ntr.y, ntr.z);
            GL.glVertex3f(nbr.x, nbr.y, nbr.z);
            GL.glVertex3f(nbl.x, nbl.y, nbl.z);

            //far plane
            GL.glVertex3f(ftr.x, ftr.y, ftr.z);
            GL.glVertex3f(ftl.x, ftl.y, ftl.z);
            GL.glVertex3f(fbl.x, fbl.y, fbl.z);
            GL.glVertex3f(fbr.x, fbr.y, fbr.z);

            //bottom plane
            GL.glVertex3f(nbl.x, nbl.y, nbl.z);
            GL.glVertex3f(nbr.x, nbr.y, nbr.z);
            GL.glVertex3f(fbr.x, fbr.y, fbr.z);
            GL.glVertex3f(fbl.x, fbl.y, fbl.z);

            //top plane
            GL.glVertex3f(ntr.x, ntr.y, ntr.z);
            GL.glVertex3f(ntl.x, ntl.y, ntl.z);
            GL.glVertex3f(ftl.x, ftl.y, ftl.z);
            GL.glVertex3f(ftr.x, ftr.y, ftr.z);

            //left plane

            GL.glVertex3f(ntl.x, ntl.y, ntl.z);
            GL.glVertex3f(nbl.x, nbl.y, nbl.z);
            GL.glVertex3f(fbl.x, fbl.y, fbl.z);
            GL.glVertex3f(ftl.x, ftl.y, ftl.z);

            // right plane
            GL.glVertex3f(nbr.x, nbr.y, nbr.z);
            GL.glVertex3f(ntr.x, ntr.y, ntr.z);
            GL.glVertex3f(ftr.x, ftr.y, ftr.z);
            GL.glVertex3f(fbr.x, fbr.y, fbr.z);

            GL.glEnd();
            GL.glDisable(GL.GL_BLEND);
        }

        public void debugDraw()
        {            
            GL.glDisable(GL.GL_TEXTURE_2D);
            GL.glColor3f(1, 1, 0);

            // Show camera heading
            //GL.glBegin(GL.GL_LINES);
            //Vector3 pos = myCamera.getPosition();
            //pos.y -= 0.5f;
            //Vector3 targ = pos + myCamera.getLookDir() * 2.0f;
            //GL.glVertex3f(pos.x, pos.y, pos.z);
            //GL.glVertex3f(targ.x, targ.y, targ.z);
            //GL.glEnd();

            //GL.glPushMatrix();
            //GL.glTranslatef(pos.x, pos.y, pos.z);
            //GL.glScalef(0.1f, 0.1f, 0.1f);
            //GL.glPolygonMode(GL.GL_FRONT_AND_BACK, GL.GL_LINE);
            //DrawSphere(0.1f, 6, 6);
            //GL.glPolygonMode(GL.GL_FRONT_AND_BACK, GL.GL_FILL);
            //GL.glPopMatrix(); 
                                 
            drawLines();
            drawNormals();
            drawPlanes();
        }
    }
}
